﻿using System;
using System.Collections.Generic;
using System.Text;
using FIRADriverLibrary;
using RobotRace.Geometry;
using RobotRace.engine.raceengine;
using RobotRace.engine.robossConnector;
using RobotRace.utils.graph;
using RobotRace.engine.path;

namespace RobotRace.engine.agents.implementation
{
    public abstract class AbstractPIDFollower : AbstractAgent
    {
        public override String Name { get; set; }
        public override List<FIRARobotState> ControlledRobots { get { return new List<FIRARobotState> { robot }; } }

        protected FIRARobotState robot;
        //protected List<Point2D> pathToFollow;
        protected RaceEngine engine;
        protected RobossConnector connector;
        protected Node currentNode;
        protected Point2D robotStartingPoint;

        public double maxVelocity { get; set; }
        public double startingStepSpeed { get; set; }
        //AKA Kp
        public double turningSpeedModifier { get; set; }
        //AKA Ki
        public double addedErrorWeight { get; set; }
        //AKA Kd
        public double addedDerivativeWeight { get; set; }

        //AKA Integral
        protected double sumOfErrors = 0.0;
        //previous error if angle
        protected double previousError = 0.0;
        private TimeSpan lastLap;
        private DateTime lastLapStartTime;
        private Boolean validLap = false;


        public AbstractPIDFollower(Point2D robotStartingPoint, FIRARobotState controlledRobot, List<Point2D> pathToFollow) : base(pathToFollow)
        {
            this.robot = controlledRobot;
            this.robotStartingPoint = robotStartingPoint;
            this.checkpointManagementStrategy = new ClosestCheckpointManagementStrategy();
            this.currentNode = checkpointManagementStrategy.findNextPoint(path, new Point2D(robot.x, robot.y)); //findTargetCheckpoint(robotStartingPoint, pathToFollow, currentCheckpoint);
        }

        public override void configure(RaceEngine engine, RobossConnector connector)
        {
            this.engine = engine;
            this.connector = connector;
        }

        public override void pause()
        {
            robot.leftSpeed = 0;
            robot.rightSpeed = 0;
        }

        public override void stop()
        {
            currentNode = path.getFirstNodeInSequence();
            robot.leftSpeed = 0;
            robot.rightSpeed = 0;
            validLap = false;
        }

        protected void obeyMaxVelocityLimit()
        {
            double currentMaxSpeed = Math.Max(Math.Abs(robot.leftSpeed), Math.Abs(robot.rightSpeed));
            if (currentMaxSpeed > maxVelocity)
            {
                robot.leftSpeed /= currentMaxSpeed;
                robot.rightSpeed /= currentMaxSpeed;
                robot.leftSpeed *= maxVelocity;
                robot.rightSpeed *= maxVelocity;
            }
        }



       
    }
}
